#include "Obstacles.h"


void Obstacles::processLaserData(const sensor_msgs::LaserScan& scan) {
    obstacles.clear();
    int flag = 0;
    double noise_threshold = 0.5; // 设置噪声阈值，允许的最大距离差异

    for (size_t i = 0; i < scan.ranges.size() - 1; ++i) {
        double dis = scan.ranges[i];
        double angle = scan.angle_min + scan.angle_increment * i;

        if (dis > scan.range_min && dis < scan.range_max && dis <= 4.0) {
            // 检查是否为噪声
            /* if (i > 0 && abs(dis - scan.ranges[i - 1]) > noise_threshold && i < scan.ranges.size() - 1 && abs(dis - scan.ranges[i + 1]) < noise_threshold) {
                continue; // 跳过此点，认为是噪声
            } */
            // 调整角度，使其相对于正前方
            double adjusted_angle = angle + (M_PI / 2); // 假设正前方是0度，背后是180度
            double x = dis * cos(angle);
            double y = dis * sin(angle);
            if(flag%40 == 0 || abs(dis - scan.ranges[i + 1]) > 0.2)
            {
                obstacles.push_back(std::make_pair(x, y));
                if(abs(dis - scan.ranges[i + 1]) > 0.2)flag = 0;
            } 
            flag++;
        }
    }
}

void Obstacles::odomCallback(const nav_msgs::Odometry& odom) {
    vehicle_x = odom.pose.pose.position.x;
    vehicle_y = odom.pose.pose.position.y;
    vehicle_yaw = tf2::getYaw(odom.pose.pose.orientation);
}

void Obstacles::transformToMapCoordinates(double x, double y) {
    double cos_yaw = cos(vehicle_yaw);
    double sin_yaw = sin(vehicle_yaw);

    double map_x = vehicle_x + (x * cos_yaw - y * sin_yaw);
    double map_y = vehicle_y + (x * sin_yaw + y * cos_yaw);

    std::pair<double, size_t> minDistIndex;
    double CrossProduct = 0.0;
    if(!points.empty()){
        Point A, B, C;
        minDistIndex = ShortestDistance(map_x, map_y, points);
        if(minDistIndex.second == static_cast<int>(points.size()) - 1){ //防止取下一个点的时候越界
            A.x = points[minDistIndex.second - 1].first;
            A.y = points[minDistIndex.second - 1].second;
            B.x = points[minDistIndex.second].first;
            B.y = points[minDistIndex.second].second;
        }else{
            A.x = points[minDistIndex.second].first;
            A.y = points[minDistIndex.second].second;
            B.x = points[minDistIndex.second + 1].first;
            B.y = points[minDistIndex.second + 1].second;
        }
        C.x = map_x;
        C.y = map_y;
        CrossProduct = crossProduct(A, B, C);  //向量叉积，如果CrossProduct>0，表示C在由A到B构成的直线的左侧
    }
    if(CrossProduct > 0 || (CrossProduct < 0 && !(minDistIndex.first >= 0.5 && minDistIndex.first <= 1.0))){
        map_coordinates.emplace_back(map_x, map_y);
    }
    visualizeObstacles();
}

std::pair<double, size_t> Obstacles::ShortestDistance(const double &map_x, const double &map_y, const std::vector<std::pair<double, double>> &points){
    double minDist = std::numeric_limits<double>::max();
    size_t minIndex = 0;
    for(size_t i = 0; i < points.size(); i++){
        double xDist = map_x - points[i].first;
        double yDist = map_y - points[i].second;
        double dist = std::sqrt(xDist * xDist + yDist * yDist);
        if (dist < minDist) {  // 如果找到更小的距离
            minDist = dist;  // 更新最小距离
            minIndex = i;  // 更新最小距离所对应的点的索引
        }
    }
    return std::make_pair(minDist, minIndex);
}

double Obstacles::crossProduct(Point &a, Point &b, Point &c) {
    //构建ab和ac向量，则有CrossProduct = ab *ac = (b - a) * (c - a) = ((b.x - a.x), (b.y - a.y)) * ((c.x - a.x), (c.y - a.y)) = (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x)
    return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
}


std::vector<Point> Obstacles::getObstaclesAsPointers() {
    std::vector<Point> obstacles_ptr;
    for (const auto& coord : map_coordinates) {
        Point points;
        points.x = coord.first;
        points.y = coord.second;
        obstacles_ptr.push_back(points);
    }
    return obstacles_ptr;
}

// 激光数据回调函数
void Obstacles::laserCallback(const sensor_msgs::LaserScan& scan) {
    // 调用processLaserData函数处理激光数据
    map_coordinates.clear();
    if(points.size() > 0 && !points.empty()){
        processLaserData(scan);
        int obstacleCount = 0;
        for (const auto& obstacle : obstacles) {
            transformToMapCoordinates(obstacle.first, obstacle.second);
            obstacleCount++;
        }
    }
}

void Obstacles::getGlobalPoints(const std::vector<std::pair<double, double>>& xy_points, const int &index){
    points.clear(); 
    size_t num_points = xy_points.size();
    if (num_points < 36) {
        // 如果元素个数小于36，选择全部点
        points = xy_points;
        return;
    }

    if (index <= 5) {
        // 选择前36个点
        points.insert(points.end(), xy_points.begin(), xy_points.begin() + 36);
    } else if (index > static_cast<int>(num_points - 36)) {
        // 选择最后36个点
        points.insert(points.end(), xy_points.end() - 36, xy_points.end());
    } else {
        // 选择以index为中心的36个点，向前6个，向后30个
        points.insert(points.end(), xy_points.begin() + index - 6, xy_points.begin() + index + 30);
    }
}

void Obstacles::visualizeObstacles() {
    visualization_msgs::Marker points;
    points.header.frame_id = "map";  // 设置参考系为地图
    points.header.stamp = ros::Time::now();
    points.ns = "Obstacles";
    points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = 1.0;

    points.id = 0;

    points.type = visualization_msgs::Marker::POINTS;

    // 设置点的大小
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // 设置颜色 (RGBA)
    points.color.r = 1.0f;
    points.color.g = 0.0f;
    points.color.b = 0.0f;
    points.color.a = 1.0;

    // 添加所有障碍物点到 marker
    for (const auto& coord : map_coordinates) {
        geometry_msgs::Point p;
        p.x = coord.first;
        p.y = coord.second;
        p.z = 0;
        points.points.push_back(p);
    }

    // 发布 marker
    marker_pub_.publish(points);
}

Obstacles::Obstacles() {
    ros::NodeHandle nh;
    marker_pub_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);  // 初始化可视化 publisher
}
